#include <Servo.h>

int motor1_controlPin1 = 2;
int motor1_controlPin2 = 3;
int motor1_enabledPin = 5;
int servo_pin = 11;





int motor1_speed = 0;
Servo servo;
int servo_angle = 20;

void setup()
{
  Serial.begin(9600);
/** MOTOR 1 **/
  pinMode(motor1_controlPin1,OUTPUT);
  pinMode(motor1_controlPin2,OUTPUT);  
  pinMode(motor1_enabledPin,OUTPUT);  
  
  motor1_stop();
  motor1_anticlockwise();
  
  
  /** SERVO **/
  servo.attach(servo_pin);
  
}

void loop()
{
 
 // onOffSwitchState = digitalRead(onOffswitchStateSwitchPin);
  delay(250);
motor1_setVel(250);
motor1_clockwise();
 motor1_start();
 
 
 

// servo_turn(servo_angle);
 delay(500);
 servo_turn(20);
 
 delay(500);
 servo_turn(40);
 
 delay(500);
 servo_turn(60);
 
 delay(500);
 servo_turn(80);
 
 delay(500);
 servo_turn(100);
 
 delay(500);
 servo_turn(120);
 
  delay(500);
 servo_turn(140);
 
  delay(500);
 servo_turn(160);
 
 
 
  
  
  
}


void motor1_stop()
{
  analogWrite(motor1_enabledPin, 0);
}

void motor1_start()
{
  analogWrite(motor1_enabledPin, motor1_speed);
}

void motor1_setVel(int v)
{
  motor1_speed=v;
 // analogWrite(motor1_enabledPin, motor1_speed);
}

void motor1_clockwise()
{
  digitalWrite(motor1_controlPin2,LOW);
  digitalWrite(motor1_controlPin1,HIGH);
}

void motor1_anticlockwise()
{
  digitalWrite(motor1_controlPin2,HIGH);
  digitalWrite(motor1_controlPin1,LOW);
}

void servo_turn(int angle)
{
  if(abs(servo.read()-angle)>1)
  {
  servo_angle = angle%180;
  servo.write(servo_angle);
  }
  
}
